/*
 * void SetMotor()        --使四个轮子按照PWM数组的值转动
 * void Stop();           --停止
 * void GoForward(int t)  --前进t毫秒
 * void GoBack(int t)     --后退t
 * void TurnLeft(int t)   --左转t
 * void TurnRight(int t)  --右转t
 * void InitCarPinSet()   --初始化
 * void SetArm(int Ang)   --机臂上下转动，范围（50，130） （低，高）
 * void SetHand(int Ang)  --抓取机构，90-开，130-合
 * int GrayRead()         --1-左边读到黑，2-两边都不黑，3-右边读到黑，4-两边读到黑
 * float GetDistence()  //   Echo = A3,Trig = D2
 */
#include <Servo.h>

Servo LF;
Servo RF;
Servo LB;
Servo RB;
Servo ArmR;
Servo ArmL;
Servo HandR;
Servo HandL;
Servo SServo;
int GrayL;
int GrayR;
int PWM[4]; //LF,LB,RF,RB
int Mov;
int ValG;   //灰度传感器情况
int cat;  //机械臂是否夹取物品
float Distence;

void setup() {
  // put your setup code here, to run once:
  InitCarPinSet();
  Mov = 1;
  ValG = 2;
  cat = 0;
  Distence = 100;
  pinMode(A0,INPUT);
  pinMode(A2,INPUT);
  pinMode(A3,INPUT);
  pinMode(2,OUTPUT);
  pinMode(4,OUTPUT);
}

void InitCarPinSet()
{
  LF.attach(7);
  LB.attach(11);
  RF.attach(8);
  RB.attach(12);
  SServo.attach(3);
  ArmL.attach(6);
  ArmR.attach(5);
  HandR.attach(10);
  HandL.attach(9);
  /*SL -> A2
   *SR -> A0
   *声纳：
   *Echo = A3
   *Trig = D2
   *
   */
  
  Stop();
  ArmR.write(90);
  ArmL.write(90);
  HandR.write(90);
  HandL.write(90);
  SServo.write(92);
  GrayL = 200;
  GrayR = 200;
}

void loop() {
  // put your main code here, to run repeatedly:
  while(Mov)
  {
    ValG = GrayRead();
    switch (ValG)
    {
      case 2: //两边都没碰到黑线，直走
      GoForward1(50);
      break;

      case 1: //左边碰到黑线，向左转一点点
      TurnLeft(50);
      break;

      case 3: //右边边碰到黑线，向右转一点点
      TurnRight(50);
      break;

      case 4: //两边碰到黑线，判断是否有夹取物
              // 有 -> 调整距离到 <8    无 -> 设置Mov = 0，小车停止任务
      {
      Stop();
      Distence = GetDistence();
      if(cat == 1)
      {
        Stop();
        digitalWrite(4,HIGH);
        delay(100);
        digitalWrite(4,LOW);
        cat = 0;
        delay(5000);
        Mov = 0;
      }
      else if(cat == 0)
      {
        if(Distence > 800) //结束工作
        {
          Mov = 0;
          Stop();
          SetArm(130);
        }
        else  //调整距离并夹取
        {
          while(Distence > 6){   //调整距离
            GoForward1(50);
            //delay(10);
            Distence = GetDistence();
          }
          Stop();
          digitalWrite(4,HIGH);
          delay(100);
          digitalWrite(4,LOW);
          cat = 1;
          delay(5000);
          
          ValG = GrayRead();
          while(ValG != 4)
          {
            GoBack(50);
            ValG = GrayRead();
          }
          Stop();
          delay(100);
          GoForward1(300);
          delay(150);
          ValG = 2;
        }
      }
      break;
    }
    }
    delay(50);
  }
  
  
}

void TestProcess()
{
  
}

void SetMotor()
{
  LF.write(PWM[0]);
  LB.write(PWM[1]);  
  RF.write(PWM[2]);
  RB.write(PWM[3]);
}

void Stop()
{  
  PWM[0] = 91;
  PWM[1] = 95;
  PWM[2] = 90;
  PWM[3] = 90;
  SetMotor();
}

void GoForward1(int t)  //前进t毫秒
{
 
  PWM[0] = 60;
  PWM[1] = 60;
  PWM[2] = 130;
  PWM[3] = 125;
  SetMotor();
  delay(t);
  //Stop();
}

void GoForward2(int t)  //前进t毫秒
{
 
  PWM[0] = 30;
  PWM[1] = 30;
  PWM[2] = 160;
  PWM[3] = 155;
  SetMotor();
  delay(t);
  //Stop();
}

void GoBack(int t)
{
  PWM[0] = 150;
  PWM[1] = 150;
  PWM[2] = 20;
  PWM[3] = 25;
  SetMotor();
  delay(t);
  //Stop();
}

void TurnLeft(int t)  //t = 1780时，左转直角弯
{
  PWM[0] = 130;
  PWM[1] = 130;
  PWM[2] = 130;
  PWM[3] = 130;
  SetMotor();
  delay(t);
  //Stop();
}

void TurnRight(int t)  //t = 1780时，右转直角弯
{
  PWM[0] = 70;
  PWM[1] = 70;
  PWM[2] = 50;
  PWM[3] = 50;
  SetMotor();
  delay(t);
  //Stop();
}

void SetArm(int Ang)
{
  ArmR.write(Ang);
  ArmL.write(180-Ang);
}

void SetHand(int Ang)
{
  HandR.write(Ang);
  HandL.write(180-Ang);
}

void Catch()
{
  /*
  SetHand(130);
  SetHand(90);  //高举，打开
  delay(200);
  
  SetArm(50);
  delay(100);
  SetHand(130); //放低，抓取
  delay(100);

  SetArm (130); //举起
  delay(100);*/
  
}

void unCatch()
{
  SetArm(50);
  delay(100);
  SetHand(90); //放低，释放
  delay(100);

  SetArm(130); //举起
}

int GrayRead() //1-左边读到黑，2-两边都不黑，3-右边读到黑，4-两边读到黑
{ //灰度值<150 ->  碰到黑线
  GrayL = analogRead(A2);
  GrayR = analogRead(A0);
  if(GrayL < 150)
  {
    if(GrayR <150)
    {
      return 4;
    }
    else
    {
      return 1;
    }
  }
  else
  {
    if(GrayR <150)
    {
      return 3;
    }
    else
    {
      return 2;
    }
  }
}

float GetDistence()  //   Echo = A3,Trig = D2
{
  float Dis = 0;
  digitalWrite(2,LOW);delayMicroseconds(2);
  digitalWrite(2,HIGH);delayMicroseconds(10);
  digitalWrite(2,LOW);
  Dis = (pulseIn(A3,HIGH)/2)*340/10000;
  
  return Dis;
}
